﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using OpenCvSharp;
using OpenCvSharp.XFeatures2D;
namespace OpenCvSharpExtension
{
    /// <summary>
    /// KAZE图片特征匹配工具
    /// </summary>
    /// <remarks>代码修改自OpenCvSharp4的KAZE-DEMO</remarks>
    public class KAZEMatchHelper
    {
        public static Point2d Point2fToPoint2d(Point2f pf)
        {
            return new Point2d(((int)pf.X), ((int)pf.Y));
        }

        public static Point Point2dToPoint(Point2d p2d)
        {
            return new Point(p2d.X, p2d.Y);
        }
        public static Point[] Point2dToPoint(Point2d[] p2d)
        {
            if (p2d == null)
            {
                return null;
            }
            Point[] ps = new Point[p2d.Length];
            for (int i = 0; i < p2d.Length; i++)
            {
                ps[i] = Point2dToPoint(p2d[i]);
            }
            return ps;
        }

        /// <summary>
        /// 使用KAZE/AKAZE算法匹配图像中的模板
        /// </summary>
        /// <param name="frame">源图像</param>
        /// <param name="temp">模板</param>
        /// <param name="useGray">是否使用灰度模式</param>
        /// <param name="drawTest"></param>
        /// <returns></returns>
        public static Point[] Match(Mat frame, Mat temp, bool useGray = false, bool fast = false)
        {
            try
            {
                Mat tempImg;
                Mat frameImg;
                if (useGray)
                {
                    tempImg = temp.CvtColor(ColorConversionCodes.BGR2GRAY);
                    frameImg = frame.CvtColor(ColorConversionCodes.BGR2GRAY);
                }
                else
                {
                    tempImg = temp;
                    frameImg = frame;
                }
                Feature2D kaze;
                if (fast)
                {//快速模式使用AKAZE算法
                    kaze = AKAZE.Create();
                }
                else
                {
                    kaze = KAZE.Create();
                }
                using (var tempDescriptors = new Mat())
                using (var frameDescriptors = new Mat())
                using (var matcher = new BFMatcher(NormTypes.L2SQR))
                //using (var matcher = new FlannBasedMatcher())//使用AKAZE时FlannBasedMatcher无效

                using (kaze)
                {
                    KeyPoint[] tempKeypoints, frameKeypoints;
                    kaze.DetectAndCompute(tempImg, null, out tempKeypoints, tempDescriptors);
                    kaze.DetectAndCompute(frameImg, null, out frameKeypoints, frameDescriptors);

                    DMatch[][] matches = matcher.KnnMatch(tempDescriptors, frameDescriptors, 2);
                    using (Mat mask = new Mat(matches.Length, 1, MatType.CV_8U))
                    {
                        mask.SetTo(new Scalar(255));
                        int nonZero = Cv2.CountNonZero(mask);
                        VoteForUniqueness(matches, mask);
                        nonZero = Cv2.CountNonZero(mask);
                        nonZero = VoteForSizeAndOrientation(frameKeypoints, tempKeypoints, matches, mask, 1.5f, 20);

                        List<Point2f> obj = new List<Point2f>();
                        List<Point2f> scene = new List<Point2f>();
                        List<DMatch> goodMatchesList = new List<DMatch>();
                        //iterate through the mask only pulling out nonzero items because they're matches
                        for (int i = 0; i < mask.Rows; i++)
                        {
                            MatIndexer<byte> maskIndexer = mask.GetGenericIndexer<byte>();
                            if (maskIndexer[i] > 0)
                            {
                                obj.Add(tempKeypoints[matches[i][0].QueryIdx].Pt);
                                scene.Add(frameKeypoints[matches[i][0].TrainIdx].Pt);
                                goodMatchesList.Add(matches[i][0]);
                            }
                        }

                        List<Point2d> objPts = obj.ConvertAll(Point2fToPoint2d);
                        List<Point2d> scenePts = scene.ConvertAll(Point2fToPoint2d);
                        if (nonZero >= 4)
                        {
                            Mat homography = Cv2.FindHomography(objPts, scenePts, HomographyMethods.Ransac, 1.5, mask);
                            nonZero = Cv2.CountNonZero(mask);

                            if (homography != null)
                            {
                                Point2f[] objCorners = { new Point2f(0, 0),
                                      new Point2f(tempImg.Cols, 0),
                                      new Point2f(tempImg.Cols, tempImg.Rows),
                                      new Point2f(0, tempImg.Rows) };

                                Point2d[] sceneCorners = MyPerspectiveTransform3(objCorners, homography);
                                return Point2dToPoint(sceneCorners);

                                //绘制调试图形
                                /*
                                //This is a good concat horizontal
                                Mat img3 = new Mat(Math.Max(tempImg.Height, frameImg.Height), frameImg.Width + tempImg.Width, MatType.CV_8UC3);
                                using (Mat left = new Mat(img3, new Rect(0, 0, tempImg.Width, tempImg.Height)))
                                using (Mat right = new Mat(img3, new Rect(tempImg.Width, 0, frameImg.Width, frameImg.Height)))
                                {
                                    tempImg.CopyTo(left);
                                    frameImg.CopyTo(right);

                                    byte[] maskBytes = new byte[mask.Rows * mask.Cols];
                                    mask.GetArray(0, 0, maskBytes);
                                    Cv2.DrawMatches(tempImg, keypoints1, frameImg, keypoints2, goodMatchesList, img3, Scalar.All(-1), Scalar.All(-1), maskBytes, DrawMatchesFlags.NotDrawSinglePoints);

                                    List<List<Point>> listOfListOfPoint2D = new List<List<Point>>();
                                    List<Point> listOfPoint2D = new List<Point>();
                                    listOfPoint2D.Add(new Point(sceneCorners[0].X + tempImg.Cols, sceneCorners[0].Y));
                                    listOfPoint2D.Add(new Point(sceneCorners[1].X + tempImg.Cols, sceneCorners[1].Y));
                                    listOfPoint2D.Add(new Point(sceneCorners[2].X + tempImg.Cols, sceneCorners[2].Y));
                                    listOfPoint2D.Add(new Point(sceneCorners[3].X + tempImg.Cols, sceneCorners[3].Y));
                                    listOfListOfPoint2D.Add(listOfPoint2D);
                                    img3.Polylines(listOfListOfPoint2D, true, Scalar.LimeGreen, 2);

                                    //This works too
                                    //Cv2.Line(img3, scene_corners[0] + new Point2d(img1.Cols, 0), scene_corners[1] + new Point2d(img1.Cols, 0), Scalar.LimeGreen);
                                    //Cv2.Line(img3, scene_corners[1] + new Point2d(img1.Cols, 0), scene_corners[2] + new Point2d(img1.Cols, 0), Scalar.LimeGreen);
                                    //Cv2.Line(img3, scene_corners[2] + new Point2d(img1.Cols, 0), scene_corners[3] + new Point2d(img1.Cols, 0), Scalar.LimeGreen);
                                    //Cv2.Line(img3, scene_corners[3] + new Point2d(img1.Cols, 0), scene_corners[0] + new Point2d(img1.Cols, 0), Scalar.LimeGreen);

                                    //img3.SaveImage("Kaze_Output.png");
                                    //Window.ShowImages(img3); 
                                   
                                  } */

                            }
                        }
                        return null;
                    }
                }
            }
            catch (Exception)
            {
                return null;
            }
        }

        // to avoid opencvsharp's bug
        static Point2d[] MyPerspectiveTransform1(Point2f[] yourData, Mat transformationMatrix)
        {
            using (Mat src = new Mat(yourData.Length, 1, MatType.CV_32FC2, yourData))
            using (Mat dst = new Mat())
            {
                Cv2.PerspectiveTransform(src, dst, transformationMatrix);
                Point2f[] dstArray = new Point2f[dst.Rows * dst.Cols];
                dst.GetArray(out  dstArray);
                Point2d[] result = Array.ConvertAll(dstArray, Point2fToPoint2d);
                return result;
            }
        }

        // fixed FromArray behavior
        static Point2d[] MyPerspectiveTransform2(Point2f[] yourData, Mat transformationMatrix)
        {
            using (var s = Mat<Point2f>.FromArray(yourData))
            using (var d = new Mat<Point2f>())
            {
                Cv2.PerspectiveTransform(s, d, transformationMatrix);
                Point2f[] f = d.ToArray();
                return f.Select(Point2fToPoint2d).ToArray();
            }
        }

        // new API
        static Point2d[] MyPerspectiveTransform3(Point2f[] yourData, Mat transformationMatrix)
        {
            Point2f[] ret = Cv2.PerspectiveTransform(yourData, transformationMatrix);
            return ret.Select(Point2fToPoint2d).ToArray();
        }

        static int VoteForSizeAndOrientation(KeyPoint[] modelKeyPoints, KeyPoint[] observedKeyPoints, DMatch[][] matches, Mat mask, float scaleIncrement, int rotationBins)
        {
            int idx = 0;
            int nonZeroCount = 0;
            byte[] maskMat = new byte[mask.Rows];
            GCHandle maskHandle = GCHandle.Alloc(maskMat, GCHandleType.Pinned);
            using (Mat m = new Mat(mask.Rows, 1, MatType.CV_8U, maskHandle.AddrOfPinnedObject()))
            {
                mask.CopyTo(m);
                List<float> logScale = new List<float>();
                List<float> rotations = new List<float>();
                double s, maxS, minS, r;
                maxS = -1.0e-10f; minS = 1.0e10f;

                //if you get an exception here, it's because you're passing in the model and observed keypoints backwards.  Just switch the order.
                for (int i = 0; i < maskMat.Length; i++)
                {
                    if (maskMat[i] > 0)
                    {
                        KeyPoint observedKeyPoint = observedKeyPoints[i];
                        KeyPoint modelKeyPoint = modelKeyPoints[matches[i][0].TrainIdx];
                        s = Math.Log10(observedKeyPoint.Size / modelKeyPoint.Size);
                        logScale.Add((float)s);
                        maxS = s > maxS ? s : maxS;
                        minS = s < minS ? s : minS;

                        r = observedKeyPoint.Angle - modelKeyPoint.Angle;
                        r = r < 0.0f ? r + 360.0f : r;
                        rotations.Add((float)r);
                    }
                }

                int scaleBinSize = (int)Math.Ceiling((maxS - minS) / Math.Log10(scaleIncrement));
                if (scaleBinSize < 2)
                    scaleBinSize = 2;
                float[] scaleRanges = { (float)minS, (float)(minS + scaleBinSize + Math.Log10(scaleIncrement)) };

                using (var scalesMat = new Mat<float>(rows: logScale.Count, cols: 1, data: logScale.ToArray()))
                using (var rotationsMat = new Mat<float>(rows: rotations.Count, cols: 1, data: rotations.ToArray()))
                using (var flagsMat = new Mat<float>(logScale.Count, 1))
                using (Mat hist = new Mat())
                {
                    flagsMat.SetTo(new Scalar(0.0f));
                    float[] flagsMatFloat1 = flagsMat.ToArray();

                    int[] histSize = { scaleBinSize, rotationBins };
                    float[] rotationRanges = { 0.0f, 360.0f };
                    int[] channels = { 0, 1 };
                    Rangef[] ranges = { new Rangef(scaleRanges[0], scaleRanges[1]), new Rangef(rotations.Min(), rotations.Max()) };
                    double minVal, maxVal;

                    Mat[] arrs = { scalesMat, rotationsMat };
                    Cv2.CalcHist(arrs, channels, null, hist, 2, histSize, ranges);
                    Cv2.MinMaxLoc(hist, out minVal, out maxVal);

                    Cv2.Threshold(hist, hist, maxVal * 0.5, 0, ThresholdTypes.Tozero);
                    Cv2.CalcBackProject(arrs, channels, hist, flagsMat, ranges);

                    MatIndexer<float> flagsMatIndexer = flagsMat.GetIndexer();

                    for (int i = 0; i < maskMat.Length; i++)
                    {
                        if (maskMat[i] > 0)
                        {
                            if (flagsMatIndexer[idx++] != 0.0f)
                            {
                                nonZeroCount++;
                            }
                            else
                                maskMat[i] = 0;
                        }
                    }
                    m.CopyTo(mask);
                }
            }
            maskHandle.Free();

            return nonZeroCount;
        }

        private static void VoteForUniqueness(DMatch[][] matches, Mat mask, float uniqnessThreshold = 0.80f)
        {
            byte[] maskData = new byte[matches.Length];
            GCHandle maskHandle = GCHandle.Alloc(maskData, GCHandleType.Pinned);
            using (Mat m = new Mat(matches.Length, 1, MatType.CV_8U, maskHandle.AddrOfPinnedObject()))
            {
                mask.CopyTo(m);
                for (int i = 0; i < matches.Length; i++)
                {
                    //This is also known as NNDR Nearest Neighbor Distance Ratio
                    if ((matches[i][0].Distance / matches[i][1].Distance) <= uniqnessThreshold)
                        maskData[i] = 255;
                    else
                        maskData[i] = 0;
                }
                m.CopyTo(mask);
            }
            maskHandle.Free();
        }
    }
}
